//+--------------------------------------------------------------------------------
//| Copyright (c) 2012 - 2013 ### an.vuongngocduy
//| This file is a part of Compact Game Engine (CGE)
//+--------------------------------------------------------------------------------
#ifndef __CGE_QUATERNION_H__
#define __CGE_QUATERNION_H__

#include "cgeCommons.h"
#include "cgeMath.h"
#include "cgeVector3.h"
#include "cgeMatrix4.h"

namespace cge
{
	namespace std
	{
		template<class TVal>
		class Matrix4;

		//! Normalized quaternion is useful for representing rotation in 3d space 
		template<class TVal>
		class Quaternion
		{
		public:
			//! Default constructor
			Quaternion()
				: X(0), Y(0), Z(0), W(0)
			{}

			
			//! Construct quaternion form specified values
			Quaternion(TVal x, TVal y, TVal z, TVal w)
				: X(x), Y(y), Z(z), W(w)
			{}


			//! Construct unit quaternion represent rotation from axis-angle
			Quaternion(const Vector3<TVal>& v3, f32 angle)
			{
				Set(v3, angle);
			}


			//! Destructor
			~Quaternion(){}


			//! Get quaternion dot product
			TVal Dot(const Quaternion& q) const
			{
				return (W*q.W - (X*q.X + Y*q.Y + Z*q.Z));
			}


			//! Get the magnitude of quaternion
			f32 Magnitude() const
			{
				return Math::Sqrt(X*X + Y*Y + Z*Z + W*W);
			}

			
			//! Get squared magnitude of quaternion
			f32 MagnitudeSquared() const
			{
				return X*X + Y*Y + Z*Z + W*W;
			}


			//! Set real-number elements of quaternion
			Quaternion& Set(TVal x, TVal y, TVal z, TVal w)
			{
				X = x;	Y = y;	Z = z;	W = w;
				return *this;
			}


			//! Set quaternion elements base on axis-angle rotation
			Quaternion& FromAxisAngle(const Vector3<TVal>& v3, f32 angle)
			{
				Vector3<TVal> nv3 = v3.Normalize();
				f32 cosHalfAngle = Math::Cos(angle / 2.0f);
				f32 sinHalfAngle = Math::Sin(angle / 2.0f);
				W = cosHalfAngle;
				X = sinHalfAngle * nv3.X;
				Y = sinHalfAngle * nv3.Y;
				Z = sinHalfAngle * nv3.Z;
				return *this;
			}


			//! Set quaternion elements base on Euler's angles
			Quaternion& FromEulerAngles(f32 ax, f32 ay, f32 az)
			{
				f32 halfAngle = ax / 2.0f;
				f32 cx = Math::Cos(halfAngle);
				f32 sx = Math::Sin(halfAngle);
				
				halfAngle = ay / 2.0f;
				f32 cy = Math::Cos(halfAngle);
				f32 sy = Math::Sin(halfAngle);
				
				halfAngle = az / 2.0f;
				f32 cz = Math::Cos(halfAngle);
				f32 sz = Math::Sin(halfAngle);
				
				W = cx*cy*cz - sx*sy*sz;
				X = cx*sy*sz + sx*cy*cz;
				Y = cx*sy*cz - sx*cy*sz;
				Z = cx*cy*sz + sx*sy*cz;

				return Normalized();
			}


			//! Set quaternion elements base on Euler's angles
			template<class TArg>
			Quaternion& FromRotationMatrix(const Matrix4<TArg>& m4)
			{
				f32 mtrace = m4.M00 + m4.M11 + m4.M22 + 1.0f;

				if(mtrace > 0.0f)
				{
					f32 scale = Math::Sqrt(mtrace);
					
					W = scale * 0.5f;
					X = ( m4.M12 - m4.M21 ) / scale;
					Y = ( m4.M20 - m4.M02 ) / scale;
					Z = ( m4.M10 - m4.M01 ) / scale;
				}
				else
				{
					if(m4.M00 > m4.M11 && m4.M00 > m4.M22)
					{
						f32 scale = Math::Sqrt(m4.M00 - m4.M11 - m4.M22 + 1.0f);
						
						X = scale * 0.5f;
						Y = ( m4.M01 + m4.M10 ) / scale;
						Z = ( m4.M02 + m4.M20 ) / scale;
						W = ( m4.M12 - m4.M21 ) / scale;
					}
					else if(m4.M11 > m4.M22)
					{
						f32 scale = Math::Sqrt(m4.M11 - m4.M00 - m4.M22 + 1.0f);

						Y = scale * 0.5f;
						X = ( m4.M01 + m4.M10 ) / scale;
						Z = ( m4.M12 + m4.M21 ) / scale;
						W = ( m4.M20 - m4.M02 ) / scale;
					}
					else
					{
						f32 scale = Math::Sqrt(m4.M22 - m4.M00 - m4.M11 + 1.0f);

						Z = scale * 0.5f;
						X = ( m4.M02 + m4.M20 ) / scale;
						Y = ( m4.M12 + m4.M21 ) / scale;
						W = ( m4.M01 - m4.M10 ) / scale;
					}
				}

				return Normalized();
			}


			//! Get rotation matrix from current quaternion
			template<class TArg>
			void ToRotationMatrix(Matrix4<TArg>& m4) const
			{
				m4.Set(
					1.0f - 2.0f*(Y*Y + Z*Z), 2.0f*(X*Y + Z*W),		  2.0f*(X*Z - Y*W),		   0.0f,
					2.0f*(X*Y - Z*W),		 1.0f - 2.0f*(X*X + Z*Z), 2.0f*(Z*Y + X*W),		   0.0f,
					2.0f*(X*Z + Y*W),		 2.0f*(Z*Y - X*W),		  1.0f - 2.0f*(X*X + Y*Y), 0.0f,
					0,						 0,						  0,					   1
				);
			}


			//! Normalize current quaternion
			Quaternion& Normalized()
			{
				f32 m = Magnitude();
				X /= m; Y /= m; Z /= m; W /= m;
				return *this;
			}


			//! Get normalize version of quaternion
			Quaternion Normalize() const
			{
				f32 m = Magnitude();
				return Quaternion(X/m, Y/m, Z/m, W/m);
			}


			//! Return conjugate quaternion
			Quaternion Conjugate() const
			{
				return Quaternion(-X, -Y, -Z, W);
			}


			//! Return inverse of current quaternion
			Quaternion Inverse() const
			{
				f32 sm = MagnitudeSquared();
				return Quaternion(-X/sm, -Y/sm, -Z/sm, W/sm);
			}


			//! Calculate the quaternion represents linear interpolation of 2 quaternions 
			Quaternion Nlerp(const Quaternion& q, f32 time, b8 shortestRotation = true) const
			{		
				Quaternion ret;
				f32 cosAngle = Dot(q);
				f32 delta = 1.0f - time;
				if ( shortestRotation && cosAngle < 0.0f )
					ret.Set(X*delta + q.X*-time, Y*delta + q.Y*-time, Z*delta + q.Z*-time, W*delta + q.W*-time);
				else
					ret.Set(X*delta + q.X*time, Y*delta + q.Y*time, Z*delta + q.Z*time, W*delta + q.W*time);
				return ret.Normalized();
			}


			//! Calculate the quaternion represents spherical linear interpolation of 2 quaternions
			Quaternion Slerp(const Quaternion& q, f32 time, f32 threshold = 0.01f, b8 shortestRotation = true) const
			{
				Quaternion ret(*this);
				f32 cosAngle = ret.Dot(q);
				
				if(cosAngle < 0.0f && shortestRotation)
				{
					cosAngle *= -1.0f;
					ret *= -1.0f;
				}
				
				if(1.0f - Math::Abs(cosAngle) >= threshold)
				{
					f32 angle = Math::Acos(cosAngle);
					f32 sinAngle = Math::Sqrt(1 -  cosAngle*cosAngle);
					f32 invSinAngle = 1.0f / sinAngle;
					f32 ratioA = Math::Sin((1.0f - time) * angle) * invSinAngle;
					f32 ratioB = Math::Sin(time * angle) * invSinAngle;
					ret = ratioA*ret + ratioB*q;
				}
				else
				{
					ret = ret + time * (q - ret); // use lerp instead
				}
				return ret;
			}

			
			//! Add two quaternions together
			Quaternion operator + (const Quaternion& q) const
			{
				return Quaternion(X + q.X, Y + q.Y, Z + q.Z, W + q.W);
			}


			//! Add two quaternions together
			Quaternion operator - (const Quaternion& q) const
			{
				return Quaternion(X - q.X, Y - q.Y, Z - q.Z, W - q.W);
			}


			//! Multiply two quaternion together
			Quaternion operator * (const Quaternion& q) const
			{
				TVal retX = X*q.W + W*q.X + Y*q.Z - Z*q.Y;
				TVal retY = Y*q.W + W*q.Y + Z*q.X - X*q.Z;
				TVal retZ = Z*q.Z + W*q.Z + X*q.Y - Y*q.X;
				TVal retW = W*q.W - X*q.X - Y*q.Y - Z*q.Z;
				return Quaternion(retX, retY, retZ, retW);
			}


			//! Multiply quaternion with a scalar
			template<class TArg>
			Quaternion& operator *= (TArg scalar)
			{
				X *= scalar; Y *= scalar; Z *= scalar; W *= scalar;
				return *this;
			};


			//! Compare two quaternions . Return true if 2 quaternions have same values 
			b8 operator == (const Quaternion<TVal>& q) const
			{
				return (X == q.X) && (Y == q.Y) && (Z == q.Z) && (W == q.W);
			}


			//! Check whether two quaternions are different
			b8 operator != (const Quaternion<TVal>& q) const
			{
				return !(*this == q);
			}


		public:
			union
			{
				struct { TVal X, Y, Z, W; };
				TVal Elements[4];
			};
		};


		//! Floating number quaternion
		typedef Quaternion<f32> Quaternionf;


		//! Double precision floating number quaternion
		typedef Quaternion<f64> Quaterniond;
	}
}

#endif//__CGE_QUATERNION_H__